#include <stdio.h>
#include "system.h"

//#define UART_USE_IRQ 1

//__asm__ __volatile__("stash %[input1]"::[input1]"r"(newObj));
//  __asm__ __volatile__("stashi 33");

void abort(void)
{
  while(1);
}
//TODO 要加上sdram的偏移，和考虑私有空间的情况

//0~0x3000
#ifdef ISBIOS
#define POS_MALLOC_INDEX 0x0
#define BIOS_RAM_START 0x10000
#define malloc_index ( *( (int*)(BIOSRAM + POS_MALLOC_INDEX) ) )
#else
void* __eof__();
tlsf_t tlsf;
void* endofMem;
#endif

#ifdef ISBIOS
#define POS_RND_SEED 0x8
#define __rnd_seed__ ( *( (unsigned long long*)(BIOSRAM + POS_RND_SEED) ) )
#else
unsigned long long __rnd_seed__;
#endif

#ifdef ISBIOS
#define UART_FIFO_SIZE 0xFFF
#define POS_UART_FIFO 0x20
#define uart_snd_FIFObtm ( *( (int*)(BIOSRAM + POS_UART_FIFO + 0x00) ) )
#define uart_rec_FIFObtm ( *( (int*)(BIOSRAM + POS_UART_FIFO + 0x04) ) )
#define uart_snd_FIFOidx ( *( (int*)(BIOSRAM + POS_UART_FIFO + 0x08) ) )
#define uart_rec_FIFOidx ( *( (int*)(BIOSRAM + POS_UART_FIFO + 0x0C) ) )
#define uart_snd_FIFO    (   (char*)(BIOSRAM + POS_UART_FIFO + 0x10  ) )
#define uart_rec_FIFO    (   (char*)(BIOSRAM + POS_UART_FIFO + 0x1010) )
#else
#define UART_FIFO_SIZE 1023
int uart_snd_FIFObtm;
int uart_rec_FIFObtm;
int uart_snd_FIFOidx;
int uart_rec_FIFOidx;
char uart_snd_FIFO[1024];
char uart_rec_FIFO[1024];
#endif

typedef struct {
  void* next;
  int size;
  int start;
} MemObj;

int align(int idx, int alignment){
  int left = idx & (alignment-1);
  idx = idx & ~(alignment-1);
  idx = idx + (left ? alignment : 0);
  return idx;
}

IRQ_PROC void _irq_dummy(){
  IRQ_PROLOG;
  
    //IOWR(IRQ, 0, 1);//irq 0
    IOWR(IRQ, 0, 2);//irq 1
    //IOWR(IRQ, 0, 4);//irq 2

  IRQ_EPILOG;
}

void systemInit(){
  //for(int i = 0; i <32;i++){
    int value = (int)_irq_dummy;
    int pos = 1;
    __asm__ __volatile__("setirq %[input1],%[input2]"::[input1]"r"(value),[input2]"r"(pos));//value, pos
  //}
}

void memInit(int vram_size){
  int mem_start = (int)(__eof__());
  mem_start = align(mem_start, 4);
  int mem_size = getMemSize();
  tlsf = tlsf_create_with_pool((void*)mem_start, mem_size - mem_start - vram_size);
}

void* malloc(size_t size){
  //return amalloc(0, size, 4);
  return tlsf_malloc(tlsf, size);
}
void* malloca(size_t size, size_t align){
  //return amalloc(0, size, 4);
  return tlsf_memalign(tlsf, align, size);
}

void* realloc(void* ptr, size_t size){
  //return amalloc(__ptr, size, 4);
  return tlsf_realloc(tlsf, ptr, size);
}
void free(void* p){
  //amalloc(p, 0, 4);
  tlsf_free(tlsf, p);
}


void mem_report(){
  // printf("mem report:\r\n");
  // MemObj* item = (void*)malloc_index;
  // MemObj* next = (void*)0;
  // int pos;
  // while(1){
  //   if(!item->next){
  //     next = (void*)0;
  //     break;
  //   }
  //   next = (void*)item->next;

  //   pos = (int)(&(item->start)) + item->size;
  //   pos = align(pos, 4);
  //   int freespace = (int)(next) - pos;
  //     printf("\t item: %08X, size:%d\r\n", (int)item, item->size);
  //   //printf("\t size: %d\r\n", size);
  //   if(freespace > 0){
  //     printf("\t freespace: %d\r\n", freespace);
  //   }
  //   item = next;
  // }

  // pos = (int)(&(item->start)) + item->size;
  // pos = align(pos, 4);
  // printf("\t freespace: %d\r\n", (int)endofMem - pos);

  printf("end.\r\n");
}

void refreshInsCache(){
  IOWR(CACHE_CTL, 1, 1);
  IOWR(CACHE_CTL, 1, 0);
}

void flushDataCache(){
  int status;
  int val = 0;
  __asm__ __volatile__("sti %[input1],%[input2]":[input1]"=r"(status):[input2]"r"(val));//value, pos
  int temp = IORD(CACHE_CTL, CACHE_CTL_CACHE_CFG);
  int cache_config_cache_size = (1 << ((int)(temp & 0xFF))) * 4;//DWORD count
  int cache_config_set_count = 1 << ((int)((temp >> 8) & 0xFF));
  int cache_size = cache_config_cache_size * cache_config_set_count * 2;

  for (int i = 0; i < cache_size; i += cache_config_cache_size)
  {
    int temp = IORD(i, 0);
  }

  for (int i = 0; i < cache_size; i += cache_config_cache_size)
  {
    int temp = IORD((i + cache_size), 0);
  }
  __asm__ __volatile__("sti %[input1],%[input2]":[input1]"=r"(status):[input2]"r"(status));//value, pos
}

void drawImg(void* srcAddr, int x, int y, int w, int h){
  IOWR(VGA_BLT, BLT_M_RST_FIFO, 0);
  IOWR(VGA_BLT, BLT_S_SRC_ADDR, (int)srcAddr);
  IOWR(VGA_BLT, BLT_S_DESTX, x);
  IOWR(VGA_BLT, BLT_S_WIDTH_HEIGHT, w); // w
  IOWR(VGA_BLT, BLT_S_WIDTH_HEIGHT, h); // h
  IOWR(VGA_BLT, BLT_M_DESTY_MEMCPY_REQ, y);
  //while(IORD(VGA_BLT, BLT_BUSY));
}

inline int getRGB565(int rgb888){
  int v1;
  v1 = ((rgb888>>19)&31)<<11;
  v1 |= ((rgb888>>10)&63)<<5;
  v1 |= ((rgb888>>3)&31);
  //__asm__ __volatile__("setrgb %0,%1,%2,3":"=r"(v1):"r"(v1),"r"(v2));
  //__asm__ __volatile__("add r0, r0, r0");
  return v1;
}

inline int getRGB565sep(int r, int g, int b){
  int v1;
  v1 = ((r>>3)&31)<<11;
  v1 |= ((g>>2)&63)<<5;
  v1 |= ((b>>3)&31);
  //__asm__ __volatile__("setrgb %0,%1,%2,3":"=r"(v1):"r"(v1),"r"(v2));
  //__asm__ __volatile__("add r0, r0, r0");
  return v1;
}

void paint(int x, int y, int w, int h, short color){
  paint_pattern(x, y, w, h, color, 0);
}

void paint_pattern(int x, int y, int w, int h, int color, int pattern){
  IOWR(VGA_BLT, BLT_M_RST_FIFO, 0);
  IOWR(VGA_BLT, BLT_S_PATTERN, pattern);
  IOWR(VGA_BLT, BLT_S_SETVAL, color);
  IOWR(VGA_BLT, BLT_S_DESTX, x);
  IOWR(VGA_BLT, BLT_S_WIDTH_HEIGHT, w); // w
  IOWR(VGA_BLT, BLT_S_WIDTH_HEIGHT, h); // h
  IOWR(VGA_BLT, BLT_M_DESTY_MEMSET_REQ, y);
  //while(IORD(VGA_BLT, BLT_BUSY));

}

int getSeed(){
  int v = 0;
  int i;
  int j;
  for(i=0;i<32;i++){
    for(j=0;j<20;j++){
      volatile int a = j;
    }
    v <<= 1;
    int r = IORD(RND, 0) & 1;
    v |= r;
  }
  return v;
}

#define __rnd_multiplier 0x5DEECE66DLL
#define __rnd_addend 0xBLL
#define __rnd_mask = ((1LL << 48) - 1)

void rndSetSeed(unsigned int seed) {
    __rnd_seed__ = (seed ^ __rnd_multiplier) & (((1LL << 48) - 1));
}

unsigned long long mul64bit(unsigned long long a, unsigned long long b){
  unsigned long long result = 0;
  unsigned long long aa = a;
  unsigned long long bb = b;
  int i;
  for(i=0;i<64;i++){
    if(aa & 1){
      result += bb;
    }
    aa>>=1;
    bb<<=1;
  }
  return result;
}

int rnd() {
  unsigned long long oldseed, nextseed;
  oldseed = __rnd_seed__;
  nextseed = (mul64bit(oldseed, __rnd_multiplier) + __rnd_addend) & (((1LL << 48) - 1));
  __rnd_seed__ = nextseed;
  return (unsigned int) (nextseed >> (48 - 32));
}

int setSndFifo(int val){
  int status;
  int ival = 0;
  if(uart_snd_FIFOidx+1!=uart_snd_FIFObtm){
    __asm__ __volatile__("sti %[input1],%[input2]":[input1]"=r"(status):[input2]"r"(ival));//value, pos
    uart_snd_FIFOidx++;
    uart_snd_FIFOidx&=UART_FIFO_SIZE;
    uart_snd_FIFO[uart_snd_FIFOidx]=(char)val;
    __asm__ __volatile__("sti %[input1],%[input2]":[input1]"=r"(status):[input2]"r"(status));//value, pos
    return 1;
  }
  return 0;
}

int getSndFifo(int* val){
  if(uart_snd_FIFObtm!=uart_snd_FIFOidx){
    uart_snd_FIFObtm++;
    uart_snd_FIFObtm&=UART_FIFO_SIZE;
    *val = uart_snd_FIFO[uart_snd_FIFObtm];
    return 1;
  }
  return 0;
}

//void setRecFifo(int val){
//  uart_rec_FIFOidx++;
//  uart_rec_FIFOidx&=UART_FIFO_SIZE;
//  uart_rec_FIFO[uart_rec_FIFOidx]=val;
//}
//
//int getRecFifo(){
//  //sti(0);
//  int status;
//  int ival = 0;
//  __asm__ __volatile__("sti %[input1],%[input2]":[input1]"=r"(status):[input2]"r"(ival));//value, pos
//
//  int val = 0;
//  if(uart_rec_FIFObtm!=uart_rec_FIFOidx){
//    uart_rec_FIFObtm++;
//    uart_rec_FIFObtm&=UART_FIFO_SIZE;
//    val = uart_rec_FIFO[uart_rec_FIFObtm];
//  }
//  //sti(1);
//  __asm__ __volatile__("sti %[input1],%[input2]":[input1]"=r"(status):[input2]"r"(status));//value, pos
//  return val;
//}




void uartIrqProc(){
  #ifdef UART_USE_IRQ
  int temp;
  if(getSndFifo(&temp)){
    IOWR(MYUART, 1 ,temp);
  }
  #endif
  IOWR(IRQ, 0, 2);
}


IRQ_PROC void _uart_irq_proc(){
  IRQ_PROLOG;
	//ldw	r2, %gprel(irqPtr)(gp)
	//callr	r2
  __asm__ __volatile__("call uartIrqProc");
  IRQ_EPILOG;
}

long long getTick(){
  long long low = IORD(MYTIMER, MYTIMER_LOW);
  long long hi = IORD(MYTIMER, MYTIMER_HI);
  long long tick = hi<<32 | (low & 0xFFFFFFFFl);
  return tick;
}

long long getMicros(){
  long long tick = getTick();
  tick = tick / 50;
  return tick;
}

long long getMills(){
  long long tick = getTick();
  tick = tick / 50000;
  return tick;
}

long long getSecond(){
  long long tick = getTick();
  tick = tick / 50000000;
  return tick;
}

int uart_read_timeout(int timeout){//ms
  long long t0;
  if(timeout!=-1){
    t0 = getMills();
  }
  while(1){
    if(timeout!=-1){
      long long t1 = getMills();
      if((t1-t0)>timeout){
        return -1;
      }
    }
    int tmp = IORD(MYUART, 0);
    if(tmp&0x100){
      IOWR(MYUART, 0, 0);
      return tmp & 0xFF;
    }
  }
}


int uart_read(){
  uart_read_timeout(-1);
}



int uart_write(int val){
#ifdef UART_USE_IRQ
  setSndFifo(val);
  if((IORD(MYUART, 1)) & 0x100){
    //发送中
  }else{
    //空闲
    int temp;
    if(getSndFifo(&temp)){
      IOWR(MYUART, 1 ,temp);
    }
  }
#else
  while((IORD(MYUART, 1)) & 0x100);
  IOWR(MYUART, 1 ,val);
#endif
}

void uart_irq_init(){
  uart_snd_FIFObtm = 0;
  uart_rec_FIFObtm = 0;
  uart_snd_FIFOidx = 0;
  uart_rec_FIFOidx = 0;
  int value = (int)_uart_irq_proc;
  int pos = 1;
  __asm__ __volatile__("setirq %[input1],%[input2]"::[input1]"r"(value),[input2]"r"(pos));//value, pos
}


#define dly_max 255
void SPI_CHIP_SELECT_HIGH(){
  IOWR(SOFTSPI, SOFTSPI_CS, (dly_max<<8) | 0x07);
}
void SPI_CHIP_SELECT_LOW(int chip){
  IOWR(SOFTSPI, SOFTSPI_CS, (dly_max<<8) | (0xFF & ~(1<<chip)));
}

//------------------------------------------------------------------------------
//volatile int spi_debug0;
int spiRec() {
  int tmp = IORD(SOFTSPI, SOFTSPI_READ);
  //printf("spiRec %d\r\n", tmp);
  return tmp;
}
int spiRec32() {
  int tmp = IORD(SOFTSPI, SOFTSPI_READ4);
  return tmp;
}

//volatile int spi_debug1;
//------------------------------------------------------------------------------
void spiSend(int data) {
  //printf("spiSend %d\r\n", data);
  IOWR(SOFTSPI, SOFTSPI_WRITE, data);
}



int uart_print_byte(int val){
  char* chardata = "0123456789ABCDEF";
  uart_write(chardata[(val>>4)&0x0F]);
  uart_write(chardata[(val)&0x0F]);
}


//from pc
int receiveDebugData(){
  while(1){
    int flg = IORD(DEBUGGER, DEBUGGER_FLG_SYSTEM_VER);
    if(flg&2)//debug_rx_from_pc_valid
    {
      int val = IORD(DEBUGGER, DEBUGGER_TXRX);
      return val;
    }
  }
}

//to pc
void sendDebugData(int val){
  while(1){
    int flg = IORD(DEBUGGER, DEBUGGER_FLG_SYSTEM_VER);
    if(!(flg&4))//debug_tx_to_pc_valid
    {
      IOWR(DEBUGGER, DEBUGGER_TXRX, val);
      return;
    }
  }
}

int getSystemVer(){
  return IORD(DEBUGGER, DEBUGGER_FLG_SYSTEM_VER)>>3;
}

int getSoftVer(){
  return SOFT_VER;
}


int getMemSize(){
  return IORD(CACHE_CTL, CACHE_CTL_MEM_SIZE);
}



#define DS1302_READ 1
#define DS1302_RAM 0x20
#define DS1302_TIME 0x1F



int getCMOS(int index){
  IOWR(DS1302, DS1302_CS, 1);//cs
  //dly();
  IOWR(DS1302, DS1302_DATA, 0x80 | ((DS1302_RAM+index) << 1) | DS1302_READ);//send read time
  int val = IORD(DS1302, DS1302_DATA);
  IOWR(DS1302, DS1302_CS, 0);//cs
  return val;
}

int setCMOS(int index, int val){
  IOWR(DS1302, DS1302_CS, 1);//cs
  //dly();
  IOWR(DS1302, DS1302_DATA, 0x80 | ((DS1302_RAM+index) << 1));//send read time
  IOWR(DS1302, DS1302_DATA, val);
  IOWR(DS1302, DS1302_CS, 0);//cs
  return 0;
}

int getTime(char* datetime){
  IOWR(DS1302, DS1302_CS, 1);//cs
  //dly();
  IOWR(DS1302, DS1302_DATA, 0x80 | (DS1302_TIME << 1) | DS1302_READ);//send read time
  datetime[0] = IORD(DS1302, DS1302_DATA);
  datetime[1] = IORD(DS1302, DS1302_DATA);
  datetime[2] = IORD(DS1302, DS1302_DATA);
  datetime[3] = IORD(DS1302, DS1302_DATA);
  datetime[4] = IORD(DS1302, DS1302_DATA);
  datetime[5] = IORD(DS1302, DS1302_DATA);
  datetime[6] = IORD(DS1302, DS1302_DATA);
  datetime[7] = IORD(DS1302, DS1302_DATA);
  IOWR(DS1302, DS1302_CS, 0);//cs
}

int setTime(char* datetime){
  IOWR(DS1302, DS1302_CS, 1);//cs
  //dly();
  IOWR(DS1302, DS1302_DATA, 0x80 | (DS1302_TIME << 1));
  IOWR(DS1302, DS1302_DATA, datetime[0]);
  IOWR(DS1302, DS1302_DATA, datetime[1]);
  IOWR(DS1302, DS1302_DATA, datetime[2]);
  IOWR(DS1302, DS1302_DATA, datetime[3]);
  IOWR(DS1302, DS1302_DATA, datetime[4]);
  IOWR(DS1302, DS1302_DATA, datetime[5]);
  IOWR(DS1302, DS1302_DATA, datetime[6]);
  IOWR(DS1302, DS1302_DATA, datetime[7]);
  IOWR(DS1302, DS1302_CS, 0);//cs

}

// void* mallocAll(int* retSize){
//   int idx = (int)__eof__();
//   idx += malloc_index;
//   int size = VIDEO_RAM_AT - idx;
//   *retSize = size;
//   malloc_index += size;
//   return (void*)idx;
// }

// void* amallocAll(int* retSize, int endOfRAM, int alignment){
//   int idx = (int)__eof__();
//   idx += malloc_index;

//   int left = idx & (alignment-1);
//   idx = idx & ~(alignment-1);
//   idx = idx + (left ? alignment : 0);

//   int size = endOfRAM - idx;
//   *retSize = size;
//   //malloc_index += size;
//   return (void*)idx;
// }

// void* amalloc(int size, int alignment){
//   int left = size & 3;
//   size = size & ~3;
//   size = size + ((left!=0)<<2);

  
// #ifdef ISBIOS
//   int idx = BIOSRAM + BIOS_RAM_START;
// #else
//   int idx = (int)__eof__();
// #endif

//   left = idx & (alignment-1);
//   idx = idx & ~(alignment-1);
//   idx = idx + (left ? alignment : 0);

//   idx += malloc_index;
//   malloc_index += size;
//   return (void*)idx;
// }

// void* _Znwm(int size){
//   size = (size & (~0x03))+4;
// #ifdef ISBIOS
//   int idx = BIOSRAM + BIOS_RAM_START;
// #else
//   int idx = (int)__eof__();
// #endif
//   idx += malloc_index;
//   malloc_index += size;
//   return (void*)idx;
// }

// void mfree(int size){
//   malloc_index -= size;
// }

// void* amalloc(void* p, size_t size, int alignment){
//   if(size<=0){
//     asm("hlt 1");
//     return 0;
//   }
//   //printf("amalloc: %d\r\n", size);
//   int pos;
//   size = align(size, 4);
//   MemObj* item = (void*)malloc_index;
//   MemObj* next = (void*)0;
//   while(1){
//     if(!item->next){
//       next = (void*)0;
//       break;
//     }
//     next = (void*)item->next;
//     if(p){
//       if(((void*)&(next->start)) == p){
//         if(size){
//           //判断前后有没有足够空间
//           // if(hasSpace){
//           //   //proc
//           //   return pp;
//           // }
//         }else{ // realloc / free
//           item->next = next->next;
//           return 0;
//         }
//       }
//     }else{ // malloc
//       //printf("\t next: %d\r\n", next);
//       pos = (int)(&(item->start)) + item->size + 8;
//       pos = align(pos, alignment) - 8;
//       int freespace = (int)(next) - pos;
//       //printf("\t size: %d\r\n", size);
//       //printf("\t freespace: %d\r\n", freespace);
//       if(freespace >= ((int)size + 8)){
//         break;
//       }
//     }
//     item = next;
//   }
//   if(p){ // realloc / free

//   }else{ // malloc
//     if(size){
//       pos = (int)(&(item->start)) + item->size + 8;
//       pos = align(pos, alignment) - 8;
//       MemObj* newItem = (void*)pos;
//       newItem->next = next;
//       //printf("\t newItem->next: %d\r\n", newItem->next);
//       newItem->size = size;
//       int end = ((int)(&(newItem->start))) + size;
//       if(end < (int)endofMem){
//         item->next = newItem;
//         //printf("\t item->next: %d\r\n", item->next);
//         void* ret = &(newItem->start);
//         //printf("ret: %d\r\n", ret);
//         return ret;
//       }else{
//         return 0;
//       }
//     }
//   }
// }

